//
// Created by rain on 17-11-1.
//

#include <algorithm>
#include <iostream>
#include <thread>

#include <ros/ros.h>
#include "std_msgs/String.h"
#include "include/System.h"
#include "mnorbslam_msgs/mnorbslam.h"

const std::string sUri = "mongodb://localhost:27017";
ORB_SLAM2::LoopClosing *mpLoopCloser;
ORB_SLAM2::Map *mpMap;
ORB_SLAM2::ORBVocabulary* mpVocabulary;
ORB_SLAM2::KeyFrameDatabase* mpKeyFrameDatabase;
mongocxx::instance mInstance;

void UpdatetoMap(const mnorbslam_msgs::mnorbslam &tmsg);
void InsertKeyFrame(const std_msgs::String str);
void InsertFrame(const std_msgs::String str);
void UpdateKFDb(const std_msgs::String str);

int main(int argc, char **argv)
{
    ros::init(argc, argv, "LoopClosing");
    ros::NodeHandle n("~");

    if (argc!=2)
    {
        cerr << endl << "Usage: rosrun ORB_SLAM2 RGBD path_to_Vocabulary" << endl;
        ros::shutdown();
        return 1;
    }

    string strVocFile;
    strVocFile = string(argv[1]);


    ros::Subscriber mLCupdateSubscriber = n.subscribe("/LocalMapping/LMUpdatemsgs", 100, UpdatetoMap);
    ros::Subscriber mKFInsertSubscriber = n.subscribe("/LocalMapping/FrametoLoopcloser", 100, InsertKeyFrame);
    ros::Subscriber mUpdateKFDbSubscriber = n.subscribe("/LocalMapping/UpdateKFDb", 100, UpdateKFDb);

    mpVocabulary = new ORB_SLAM2::ORBVocabulary();
    bool bVocLoad = mpVocabulary->loadFromBinaryFile(strVocFile);
    if(!bVocLoad)
    {
        cerr << "Wrong path to vocabulary. " << endl;
        cerr << "Falied to open at: " << strVocFile << endl;
        exit(-1);
    }

    mpKeyFrameDatabase = new ORB_SLAM2::KeyFrameDatabase(*mpVocabulary);
    mpMap = new ORB_SLAM2::Map(mpKeyFrameDatabase, sUri);

    mpLoopCloser = new ORB_SLAM2::LoopClosing(mpMap, mpVocabulary, ORB_SLAM2::System::RGBD !=ORB_SLAM2::System::MONOCULAR, &n);

    std::thread mptLoopClosing = thread(&ORB_SLAM2::LoopClosing::Run,mpLoopCloser);

    std::thread processUpdate(&ORB_SLAM2::LoopClosing::UpdateNewtoDatabase, mpLoopCloser);

    ros::spin();

    return 0;
}

/**
 * @brief ros callback function
 * @param tmsg
 */
void UpdatetoMap(const mnorbslam_msgs::mnorbslam &tmsg)
{
    std::pair<std::string, std::set<std::string> > mKFBuffer;
    std::pair<std::string, std::set<std::string> > mMPBuffer;

    if (tmsg.var_type=="Keyframe")
    {
        mKFBuffer.first = tmsg.var_id;
        mKFBuffer.second.insert(tmsg.var_sets.begin(), tmsg.var_sets.end());

        mpLoopCloser->Update_KFtoMap(mKFBuffer);
    }
    else if(tmsg.var_type=="MapPoint")
    {
        mMPBuffer.first = tmsg.var_id;
        mMPBuffer.second.insert(tmsg.var_sets.begin(), tmsg.var_sets.end());

        mpLoopCloser->Update_MPtoMap(mMPBuffer);
    }
}

void InsertKeyFrame(const std_msgs::String str)
{
    mpLoopCloser->InsertKeyFrame(str);
}

void UpdateKFDb(const std_msgs::String str)
{
    mpLoopCloser->UpdateKeyFrameDatabase(str);
}

